using System.Collections.Generic;
using System.Linq;

namespace RobotClient.Services
{
    /// <summary>
    /// 实时位置监控服务（线程安全）
    /// </summary>
    public class PositionMonitorService
    {
        /// <summary>
        /// 位置队列（先进先出，最大容量50）
        /// </summary>
        private readonly Queue<PositionRecord> _positionQueue = new Queue<PositionRecord>();
        
        /// <summary>
        /// 位置队列最大容量限制
        /// </summary>
        private const int MAX_QUEUE_SIZE = 50;

        /// <summary>
        /// 队列操作线程锁
        /// </summary>
        private readonly object _queueLock = new object();

        /// <summary>
        /// 维护位置队列（自动移除最早记录）
        /// </summary>
        /// <param name="newPosition">新到达的机器人位置坐标</param>
        public void MaintainPositionQueue(PositionRecord newPosition)
        {
            lock (_queueLock)
            {
                if (_positionQueue.Count >= MAX_QUEUE_SIZE)
                {
                    _positionQueue.Dequeue();
                }
                _positionQueue.Enqueue(newPosition);
            }
        }

        /// <summary>
        /// 检查是否移动了位置
        /// </summary>
        /// <param name="target">目标位置坐标</param>
        /// <returns>当最新位置与目标点三维欧式距离小于0.1米时返回true</returns>
        public Task<bool> CheckPositionReached(PositionRecord target)
        {
            lock (_queueLock)
            {
                if (_positionQueue.Count < MAX_QUEUE_SIZE) return Task.FromResult(false);
                
                return  FindMaxDistance(target);
            }
        }
        public DateTime StagnationStartTime { get; private set; } = DateTime.MinValue;

        public TimeSpan CalculateStagnationDuration()
        {
            lock (_queueLock)
            {
                return DateTime.Now - StagnationStartTime;
            }
        }
        /// <summary>
        /// 查找队列中与目标点的最大位移
        /// </summary>
        private  async Task<bool> FindMaxDistance(PositionRecord target)
        {
            return await Task.Factory.StartNew(() => {

                double maxDistance = 0;
                foreach (var position in _positionQueue)
                {
                    var distance = CalculateDistance(position, target);
                    maxDistance = System.Math.Max(maxDistance, distance);
                }
                bool istrue = maxDistance < 0.2;
                
                lock (_queueLock)
                {
                    if (istrue)
                    {
                        if (StagnationStartTime == DateTime.MinValue)
                        {
                            StagnationStartTime = DateTime.Now;
                        }
                    }
                    else
                    {
                        StagnationStartTime = DateTime.MinValue;
                    }
                }
              return istrue;
            });

        }

        /// <summary>
        /// 计算三维空间欧几里得距离
        /// </summary>
        /// <param name="a">位置点A</param>
        /// <param name="b">位置点B</param>
        /// <returns>两点间的直线距离（单位：米）</returns>
        private double CalculateDistance(PositionRecord a, PositionRecord b)
        {
            return System.Math.Sqrt(
                System.Math.Pow(a.X - b.X, 2) +
                System.Math.Pow(a.Y - b.Y, 2) +
                System.Math.Pow(a.Z - b.Z, 2));
        }
    }

    /// <summary>
    /// 机器人空间坐标记录（笛卡尔坐标系）
    /// </summary>
    /// <param name="X">X轴坐标（单位：米）</param>
    /// <param name="Y">Y轴坐标（单位：米）</param>
    /// <param name="Z">Z轴坐标（单位：米）</param>
    public record PositionRecord(double X, double Y, double Z);
}